#pragma once

#include "vec2.h"
#include <math.h>

OPEN_JLIB_NS


struct Vec3 {

  f32v4 p;

  inline Vec3 () noexcept = default;
  inline Vec3 (f32v4 p) noexcept : p(p) {
  }
  inline Vec3 (f32 x, f32 y, f32 z = 0.0F) noexcept {
    p = avx_vinsertps<0x18, f32v4>(x, y);
    p = avx_vinsertps<0x28, f32v4>(p, z);
  }
  inline Vec3 (Vec2 v, f32 z = 0.0F) noexcept {
    p = avx_vinsertps<0x28, f32v4>(v.p, z);
  }
  inline Vec3 (f32 s) noexcept {
    p = avx_vshufps<0xC0, f32v4>(s, s);
  }
  inline Vec3 (const f32* m) noexcept {
    auto b = avx_vmovq<f64>(m);
    p = avx_vinsertps<0x28, f32v4>(b, m+2);
  }


  inline f32 operator[] (uwl i) const noexcept {
    return p[i];
  }


  inline f32 x () const noexcept {
    return p[0];
  }
  inline f32 y () const noexcept {
    return p[1];
  }
  inline f32 z () const noexcept {
    return p[2];
  }


  inline Vec3& x (f32 s) noexcept {
    p[0] = s;
    return *this;
  }
  inline Vec3& y (f32 s) noexcept {
    p[1] = s;
    return *this;
  }
  inline Vec3& z (f32 s) noexcept {
    p[2] = s;
    return *this;
  }


  inline Vec3& operator+= (Vec3 v) noexcept {
    p += v.p;
    return *this;
  }
  inline Vec3& operator-= (Vec3 v) noexcept {
    p -= v.p;
    return *this;
  }
  inline Vec3& operator*= (Vec3 v) noexcept {
    p *= v.p;
    return *this;
  }
  inline Vec3& operator/= (Vec3 v) noexcept {
    p /= v.p;
    return *this;
  }


  inline Vec3& operator*= (f32 s) noexcept {
    p *= s;
    return *this;
  }
  inline Vec3& operator/= (f32 s) noexcept {
    p /= s;
    return *this;
  }


  inline Vec3& clamp (Vec3 min, Vec3 max) noexcept {
    p = avx_vmaxps<f32v4>(p, min.p);
    p = avx_vminps<f32v4>(p, max.p);
    return *this;
  }
  inline f32 normalize () noexcept {
    auto l2 = avx_vdpps<0x77, f32v4>(p, p);
    auto l = avx_vsqrtps<f32v4>(l2);
    p = avx_vdivps<f32v4>(p, l);
    return avx_cast<f32>(l);
  }


  inline Vec3 operator+ (Vec3 v) const noexcept {
    return p + v.p;
  }
  inline Vec3 operator- (Vec3 v) const noexcept {
    return p - v.p;
  }
  inline Vec3 operator* (Vec3 v) const noexcept {
    return p * v.p;
  }
  inline Vec3 operator/ (Vec3 v) const noexcept {
    return p / v.p;
  }


  inline Vec3 operator* (f32 s) const noexcept {
    return p * s;
  }
  inline Vec3 operator/ (f32 s) const noexcept {
    return p / s;
  }


  inline f32 operator& (Vec3 v) const noexcept {
    return avx_vdpps<0x71, f32>(p, v.p);
  }
  inline Vec3 operator^ (Vec3 v) const noexcept {
    auto a0 = avx_vshufps<0x09, f32v4>(p, p);
    auto b0 = avx_vshufps<0x12, f32v4>(v.p, v.p);
    auto r0 = a0 * b0;
    auto a1 = avx_vshufps<0x12, f32v4>(p, p);
    auto b1 = avx_vshufps<0x09, f32v4>(v.p, v.p);
    auto r1 = a1 * b1;
    return r0 - r1;
  }


  inline Vec3 operator+ () const noexcept {
    f32v4 t;
    auto s = avx_vpcmpeqq<f32v4>(t, t);
    auto m = avx_vpsrld<1, f32v4>(s);
    return avx_vandps<f32v4>(m, p);
  }
  inline Vec3 operator- () const noexcept {
    f32v4 t;
    auto z = avx_vxorps<f64>(t, t);
    return avx_vsubps<f32v4>(z, p);
  }
  inline Vec3 operator* () const noexcept {
    auto l2 = avx_vdpps<0x77, f32v4>(p, p);
    auto l = avx_vsqrtps<f32v4>(l2);
    return avx_vdivps<f32v4>(p, l);
  }
  inline f32 operator~ () const noexcept {
    auto l2 = avx_vdpps<0x71, f32>(p, p);
    return avx_vsqrtss<f32>(l2, l2);
  }


  inline Vec3 luminance () const noexcept {
    static const f32v4 f = {0.299F, 0.587F, 0.114F, 0.0F};
    return avx_vdpps<0x77, f32v4>(p, f);
  }


  inline static Vec3 zero () noexcept {
    f32v4 t;
    return avx_vxorps<f32v4>(t, t);
  }
  inline static Vec3 one () noexcept {
    return avx_vshufps<0xC0, f32v4>(1.0F, 1.0F);
  }
  template<u8 Index>
  inline static Vec3 unit () noexcept {
    return avx_vinsertps<Index | (0xF^(1<<Index)), f32v4>(1.0F, 1.0F);
  }


  inline static f32 distance (Vec3 a, Vec3 b) noexcept {
    return ~(b-a);
  }
  inline static Vec3 min (Vec3 a, Vec3 b) noexcept {
    return avx_vminps<f32v4>(a.p, b.p);
  }
  inline static Vec3 max (Vec3 a, Vec3 b) noexcept {
    return avx_vmaxps<f32v4>(a.p, b.p);
  }
  inline static Vec3 clamp (Vec3 v, Vec3 min, Vec3 max) noexcept {
    return Vec3::min(Vec3::max(v, min), max);
  }
  inline static Vec3 lerp (Vec3 a, Vec3 b, Vec3 t) noexcept {
    return a + (b-a)*t;
  }
  inline static Vec3 slerp (Vec3 a, Vec3 b, f32 t) noexcept {
    auto La = a.normalize();
    auto Lb = b.normalize();
    auto Lt = La + (Lb-La)*t;
    auto Ln = ~(a^b);
    auto R = asinf(Ln);
    auto r = R * t;
    auto p = sinf(R-r) / Ln;
    auto q = sinf(r) / Ln;
    return (a*p + b*q) * Lt;
  }
};


inline Vec3 operator* (f32 a, Vec3 b) noexcept {
  return b * a;
}


inline Vec3 operator"" _RGB (unsigned long long c) noexcept {
  auto i = (u32)c;
  auto s = 1.0F / 255.0F;
  auto r = ((i >> 16) & 0xFF) * s;
  auto g = ((i >> 8) & 0xFF) * s;
  auto b = (i & 0xFF) * s;
  return {r, g, b};
}

inline Vec3 operator"" _LUM (unsigned long long c) noexcept {
  auto v = ((u32)c & 0xFF) * (1.0F / 255.0F);
  return {v, v, v};
}

inline Vec3 operator"" _YUV (unsigned long long c) noexcept {
  auto i = (u32)c;
  auto s = 1.0F / 255.0F;
  auto y = ((i >> 16) & 0xFF) * s;
  auto u = ((i >> 8) & 0xFF) * s;
  auto v = (i & 0xFF) * s;
  auto r = y + 1.14F * v;
  auto g = y - 0.39F * u - 0.58F * v;
  auto b = y + 2.03F * u;
  return {r, g, b};
}

inline Vec3 operator"" _HSV (unsigned long long c) noexcept {
  auto v = (c % 100) * 0.01F;
  auto s = (c / 100 % 100) * 0.01F;
  auto h = (c / 10000 % 360) * (1.0F / 60.0F);
  auto i = (s32)h;
  auto f = h - i;
  auto x = v * (1.0F - s);
  auto y = v * (1.0F - s * f);
  auto z = v * (1.0F - s * (1.0F - f));
  auto r = 0.0F;
  auto g = 0.0F;
  auto b = 0.0F;
  switch (i) {
    case 0: r = v; g = z; b = x; break;
    case 1: r = y; g = v; b = x; break;
    case 2: r = x; g = v; b = z; break;
    case 3: r = x; g = y; b = v; break;
    case 4: r = z; g = x; b = v; break;
    case 5: r = v; g = x; b = y; break;
    default: break;
  }
  return {r, g, b};
}


CLOSE_JLIB_NS
